package com.lijian.service;

import com.lijian.model.BaseStationCoordinate;
import com.lijian.model.CoordinateData;
import gnu.io.*;
import lombok.extern.slf4j.Slf4j;

import java.io.IOException;
import java.io.InputStream;
import java.math.BigInteger;

/**
 * 打开串口 监听串口数据 计算坐标
 *
 * @author 李健
 * @date 2021/03/01
 */
@Slf4j
@SuppressWarnings("all")
public class SerialPortConnect {

    //串口名称
    private static final String SERIAL_PORT_NAME = "COM3";
    //波特率
    private static final int BAUD_RATE = 9800;
    //数据位 默认8
    private static final int DATA_BITS = SerialPort.DATABITS_8;
    //停止位 默认1
    private static final int STOP_BITS = SerialPort.STOPBITS_1;
    //校验位 默认无
    private static final int PARITY = SerialPort.PARITY_NONE;

    /**
     * 打开串口并监听数据
     */
    public void listenData(BaseStationCoordinate baseStationCoordinate, WebSocket webSocket) {
        try {
            //通过串口名称得到串口
            CommPortIdentifier portIdentifier = CommPortIdentifier.getPortIdentifier(SERIAL_PORT_NAME);
            //打开串口
            CommPort commPort = portIdentifier.open(SERIAL_PORT_NAME, 2000);
            //判断是否是串口
            if (commPort instanceof SerialPort) {
                SerialPort serialPort = (SerialPort) commPort;
                //设置串口参数
                serialPort.setSerialPortParams(BAUD_RATE, DATA_BITS, STOP_BITS, PARITY);
                System.out.println("打开串口成功，串口名称：" + SERIAL_PORT_NAME);
                //添加事件监听
                serialPort.addEventListener(event -> {
                    //数据通知
                    if (event.getEventType() == SerialPortEvent.DATA_AVAILABLE) {
                        InputStream inputStream = null;
                        byte[] bytes = null;
                        try {
                            //获得串口的输入流
                            inputStream = serialPort.getInputStream();
                            //获得数据长度
                            int buffLength = inputStream.available();
                            //根据数据长度初始化字节数组
                            bytes = new byte[buffLength];
                            //读到字节数组中
                            inputStream.read(bytes);
                        } catch (IOException e) {
                            e.printStackTrace();
                        } finally {
                            try {
                                if (inputStream != null) {
                                    inputStream.close();
                                }
                            } catch (IOException e) {
                                e.printStackTrace();
                            }
                        }
                        //解析串口数据
                        String msg = new String(bytes);
                        if (msg.startsWith("mc")) {
                            String[] split = msg.split("\r\n");
                            //截取TOF数据
                            String TOFReportMessage = split[0];
                            String[] TOF = TOFReportMessage.split(" ");
                            //距离
                            double t_a0 = hex16To10(TOF[2]);
                            double t_a1 = hex16To10(TOF[3]);
                            double t_a2 = hex16To10(TOF[4]);
                            //基站坐标
                            double a0_x = baseStationCoordinate.getA0_x();
                            double a0_y = baseStationCoordinate.getA0_y();
                            double a1_x = baseStationCoordinate.getA1_x();
                            double a1_y = baseStationCoordinate.getA1_y();
                            double a2_x = baseStationCoordinate.getA2_x();
                            double a2_y = baseStationCoordinate.getA2_y();
                            double[] coordinate = trilateration(a0_x, a0_y, t_a0, a1_x, a1_y, t_a1, a2_x, a2_y, t_a2);
                            CoordinateData data = new CoordinateData(doubleTo3(coordinate[0]), doubleTo3(coordinate[1]));
                            try {
                                webSocket.sendMessage(data);
                            } catch (Exception e) {
                                log.error("连接断开");
                            }
                            System.out.println("坐标：x=" + doubleTo3(coordinate[0]) + " y=" + doubleTo3(coordinate[1]));
                        }
                    }
                });
                //串口有数据监听
                serialPort.notifyOnDataAvailable(true);
                //中断事件监听
                serialPort.notifyOnBreakInterrupt(true);
            }
        } catch (NoSuchPortException e) {
            System.out.println("没有可用的串口");
        } catch (Exception e) {
            e.printStackTrace();
        }
    }

    /**
     * 十六进制转十进制
     *
     * @param hex16
     * @return
     */
    private static double hex16To10(String hex16) {
        return mmToM(new BigInteger(hex16, 16).intValue());
    }

    /**
     * 毫米转米
     *
     * @param mm
     * @return
     */
    private static double mmToM(int mm) {
        return doubleTo3(mm * 0.001);
    }

    /**
     * 双精度四舍五入保留三位小数
     *
     * @param d
     * @return
     */
    private static double doubleTo3(double d) {
        return (double) Math.round(d * 1000) / 1000;
    }

    /**
     * 三边测量法 计算标签坐标
     *
     * @param x1 基站A0 x坐标
     * @param y1 基站A0 y坐标
     * @param d1 基站A0到T的距离
     * @param x2 基站A1 x坐标
     * @param y2 基站A1 y坐标
     * @param d2 基站A1到T的距离
     * @param x3 基站A2 x坐标
     * @param y3 基站A2 y坐标
     * @param d3 基站A2到T的距离
     * @return
     */
    public static double[] trilateration(double x1, double y1, double d1, double x2, double y2, double d2, double x3, double y3, double d3) {
        double[] d = {0.0, 0.0};

        double a11 = 2 * (x1 - x3);

        double a12 = 2 * (y1 - y3);

        double b1 = Math.pow(x1, 2) - Math.pow(x3, 2) + Math.pow(y1, 2) - Math.pow(y3, 2) + Math.pow(d3, 2) - Math.pow(d1, 2);

        double a21 = 2 * (x2 - x3);

        double a22 = 2 * (y2 - y3);

        double b2 = Math.pow(x2, 2) - Math.pow(x3, 2) + Math.pow(y2, 2) - Math.pow(y3, 2) + Math.pow(d3, 2) - Math.pow(d2, 2);

        d[0] = (b1 * a22 - a12 * b2) / (a11 * a22 - a12 * a21);

        d[1] = (a11 * b2 - b1 * a21) / (a11 * a22 - a12 * a21);

        return d;
    }
}
